#include "ppformbuilder.h"
#include "mykroneckerproduct.h"
#include "unsupported/Eigen/KroneckerProduct"
#include <linearsolvers.h>
#include <serializer.h>
#include <iostream>
#include <utilities.h>
#include <algorithm>

namespace SPLINTER
{
    // Default constructor
    PPForm::Builder::Builder(const DataTable &data)
        : _data(data),
          _degrees(getPPFormDegrees(data.getNumVariables(), 3)),
          _numBasisFunctions(std::vector<unsigned int>(data.getNumVariables(), 0)),
          _knotSpacing(KnotSpacing::AS_SAMPLED),
          _smoothing(Smoothing::NONE),
          _alpha(0.1),
          _bondary(Boundary::NATURAL)
    {
    }

    template <typename T>
    bool allEqual(std::vector<T> const &v)
    {
        return std::adjacent_find(v.begin(), v.end(), std::not_equal_to<T>()) == v.end();
    }

    /*
     * Build ppForm
     */
    PPForm PPForm::Builder::build() const
    {
        // Check data
        // TODO: Remove this test
        if (!_data.isGridComplete())
            throw Exception("PPForm::Builder::build: Cannot create ppForm from irregular (incomplete) grid.");

        // Build knot vectors
        // std::vector<Polynomial> polynomials, std::vector< std::vector<double> > knotVectors, std::vector<unsigned int> basisDegrees
        auto knotVectors = computeKnotVectors();
        // Build ppForm (with default coefficients)
        auto ppForm = PPForm(knotVectors, _degrees);

        // Compute coefficients from samples and update ppForm
        auto polynomials = computePolynomials(ppForm);
        ppForm.setPolynomials(polynomials);

        return ppForm;
    }

    /*
     * Find coefficients of ppForm by solving:
     * min ||A*x - b||^2 + alpha*||R||^2,
     * where
     * A = mxn matrix of n basis functions evaluated at m sample points,
     * b = vector of m sample points y-values (or x-values when calculating knot averages),
     * x = ppForm coefficients (or knot averages),
     * R = Regularization matrix,
     * alpha = regularization parameter.
     */
    std::vector<Polynomial> PPForm::Builder::computePolynomials(const PPForm &ppForm) const
    {
        std::vector<Polynomial> polynomials;

        if (allEqual(_degrees))
        {
            if (_degrees[0] == 1)
            {
                std::vector<double> x;
                for (auto it = _data.cbegin(); it != _data.cend(); it++)
                {
                    x.push_back(it->getX()[0]);
                }
                auto ys = _data.getVectorY();
                int n = x.size() - 1;
                for (int i = 0; i < n; i++)
                {
                    polynomials.push_back(Polynomial(2, (x[i + 1] * ys[i] - x[i] * ys[i + 1]) / (x[i + 1] - x[i]), (ys[i + 1] - ys[i]) / (x[i + 1] - x[i])));
                }
                return polynomials;
            }
            else if (_degrees[0] == 3)
            {
                SparseMatrix B = computeBasisFunctionMatrix(ppForm);
                SparseMatrix A = B;
                DenseVector b = getSamplePointValues();

                DenseVector x;

                int numEquations = A.rows();
                int maxNumEquations = 100;
                bool solveAsDense = (numEquations < maxNumEquations);

                if (!solveAsDense)
                {
#ifndef NDEBUG
                    std::cout << "PPForm::Builder::computePPFormCoefficients: Computing ppForm control points using sparse solver." << std::endl;
#endif // NDEBUG

                    SparseLU<> s;
                    // bool successfulSolve = (s.solve(A,Bx,Cx) && s.solve(A,By,Cy));

                    solveAsDense = !s.solve(A, b, x);
                }

                if (solveAsDense)
                {
#ifndef NDEBUG
                    std::cout << "PPForm::Builder::computePPFormCoefficients: Computing ppForm control points using dense solver." << std::endl;
#endif // NDEBUG

                    DenseMatrix Ad = A.toDense();
                    DenseQR<DenseVector> s;
                    // DenseSVD<DenseVector> s;
                    // bool successfulSolve = (s.solve(Ad,Bx,Cx) && s.solve(Ad,By,Cy));
                    if (!s.solve(Ad, b, x))
                    {
                        throw Exception("PPForm::Builder::computePPFormCoefficients: Failed to solve for ppForm coefficients.");
                    }
                }
                for (int i = 0; i < x.size(); i += 4)
                {
                    DenseVector polynomial(4);
                    polynomial << x(i + 3), x(i + 2), x(i + 1), x(i);

                    polynomials.push_back(Polynomial(polynomial));
                }

                return polynomials;
            }
        }

        return polynomials;
    }

    SparseMatrix PPForm::Builder::computeBasisFunctionMatrix(const PPForm &ppForm) const
    {
        unsigned int numVariables = _data.getNumVariables();
        unsigned int numSamples = _data.getNumSamples();

        int n = numSamples - 1;
        DenseMatrix A(DenseMatrix::Zero(4 * n, 4 * n));

        std::vector<double> x;
        for (auto it = _data.cbegin(); it != _data.cend(); it++)
        {
            x.push_back(it->getX()[0]);
        }

        auto y = _data.getVectorY();

        // 端点处函数值条件
        for (int i = 0; i < n; i++)
        {
            for (int j = 0; j < 4; j++)
            {
                // 端点处
                A(2 * i, 4 * i + j) = pow(x[i], j);
                A(2 * i + 1, 4 * i + j) = pow(x[i + 1], j);
                if (i + 1 < n)
                {
                    if (j >= 1) // 端点处一阶导函数连续
                    {
                        A(2 * n + 2 * i, 4 * i + j) = j * pow(x[i + 1], j - 1);
                        A(2 * n + 2 * i, 4 * (i + 1) + j) = -j * pow(x[i + 1], j - 1);
                    }

                    if (j >= 2) // 端点处二阶导函数连续
                    {
                        A(2 * n + 2 * i + 1, 4 * i + j) = j * (j - 1) * pow(x[i + 1], j - 2);
                        A(2 * n + 2 * i + 1, 4 * (i + 1) + j) = -j * (j - 1) * pow(x[i + 1], j - 2);
                    }
                }
            }
        }

        if (_bondary == Boundary::NATURAL || _bondary == Boundary::SECOND_DERIVATIVES_AT_END)
        {
            for (int j = 2; j < 4; j++)
            {
                A(4 * n - 2, j) = j * (j - 1) * pow(x[0], j - 2);
                A(4 * n - 1, 4 * (n - 1) + j) = j * (j - 1) * pow(x[n], j - 2);
            }
        }
        else if (_bondary == Boundary::COMPLETE)
        {
            for (int j = 1; j < 4; j++)
            {
                A(4 * n - 2, j) = j * pow(x[0], j - 1);
                A(4 * n - 1, 4 * (n - 1) + j) = j * pow(x[n], j - 1);
            }
        }
        else if (_bondary == Boundary::NOT_A_KNOT)
        {
            A(4 * n - 2, 3) = 1;
            A(4 * n - 2, 7) = -1;
            A(4 * n - 1, 4 * (n - 2) + 3) = 1;
            A(4 * n - 2, 4 * (n - 1) + 3) = -1;
        }
        else if (_bondary == Boundary::PERIODIC)
        {
            for (int j = 1; j < 4; j++)
            {
                A(4 * n - 2, j) = j * pow(x[0], j - 1);
                A(4 * n - 2, 4 * (n - 1) + j) = -j * pow(x[n], j - 1);
                if (j >= 2)
                {
                    A(4 * n - 1, j) = j * (j - 1) * pow(x[0], j - 2);
                    A(4 * n - 1, 4 * (n - 1) + j) = -j * (j - 1) * pow(x[n], j - 2);
                }
            }
        }
        auto As = A.sparseView();
        // As.makeCompressed();
        return As;

        // TODO: Reserve nnz per row (degree+1)
        // int nnzPrCol = ppForm.basis.supportedPrInterval();
    }

    DenseVector PPForm::Builder::getSamplePointValues() const
    {
        int n = _data.getNumSamples() - 1;
        DenseVector b = DenseVector::Zero(4 * n);
        auto y = _data.getVectorY();

        if (_bondary == Boundary::COMPLETE)
        {
            const ISampler &d_sampler = sampler->derivation();
            auto it = _data.cbegin();
            y.push_back(d_sampler(vectorToDenseVector(it->getX()))(0));
            it = _data.cend();
            it--;
            y.push_back(d_sampler(vectorToDenseVector(it->getX()))(0));
        }
        else if (_bondary == Boundary::SECOND_DERIVATIVES_AT_END)
        {
            const ISampler &d_sampler = sampler->derivation().derivation();
            auto it = _data.cbegin();
            y.push_back(d_sampler(vectorToDenseVector(it->getX()))(0));
            it = _data.cend();
            it--;
            y.push_back(d_sampler(vectorToDenseVector(it->getX()))(0));
        }
        else if (_bondary == Boundary::PERIODIC)
        {
            y[y.size() - 1] = (*sampler)(vectorToDenseVector(_data.cbegin()->getX()))(0);
        }

        for (int i = 0; i < n; i++)
        {

            b[2 * i] = y[i];
            b[2 * i + 1] = y[i + 1];
            if (i + 1 < n)
            {
                b[2 * n + 2 * i] = 0;
                b[2 * n + 2 * i + 1] = 0;
            }
        }

        if (_bondary == Boundary::NATURAL)
        {
            b[4 * n - 2] = b[4 * n - 1] = 0;
        }
        else if (_bondary == Boundary::COMPLETE)
        {
            b[4 * n - 2] = y[n + 1];
            b[4 * n - 1] = y[n + 2];
        }
        else if (_bondary == Boundary::NOT_A_KNOT)
        {
            b[4 * n - 2] = b[4 * n - 1] = 0;
        }
        else if (_bondary == Boundary::SECOND_DERIVATIVES_AT_END)
        {
            b[4 * n - 2] = y[n + 1];
            b[4 * n - 1] = y[n + 2];
        }
        else if (_bondary == Boundary::PERIODIC)
        {
            b[4 * n - 2] = b[4 * n - 1] = 0;
        }

        return b;
    }

    // Compute all knot vectors from sample data
    std::vector<std::vector<double>> PPForm::Builder::computeKnotVectors() const
    {
        if (_data.getNumVariables() != _degrees.size())
            throw Exception("PPForm::Builder::computeKnotVectors: Inconsistent sizes on input vectors.");

        std::vector<std::vector<double>> grid;

        for (auto it = _data.cbegin(); it != _data.cend(); it++)
        {
            grid.push_back(it->getX());
        }

        std::vector<std::vector<double>> knotVectors;

        for (unsigned int i = 0; i < grid.size(); ++i)
        {
            // Compute knot vector
            knotVectors.push_back(grid.at(i));
        }

        return knotVectors;
    }

    // Compute a single knot vector from sample grid and degree
    std::vector<double> PPForm::Builder::computeKnotVector(const std::vector<double> &values,
                                                           unsigned int degree,
                                                           unsigned int numBasisFunctions) const
    {
        switch (_knotSpacing)
        {
        case KnotSpacing::AS_SAMPLED:
            return knotVectorMovingAverage(values, degree);
        case KnotSpacing::EQUIDISTANT:
            return knotVectorEquidistant(values, degree, numBasisFunctions);
        case KnotSpacing::EXPERIMENTAL:
            return knotVectorBuckets(values, degree);
        default:
            return knotVectorMovingAverage(values, degree);
        }
    }

    /*
     * Automatic construction of (p+1)-regular knot vector
     * using moving average.
     *
     * Requirement:
     * Knot vector should be of size n+p+1.
     * End knots are should be repeated p+1 times.
     *
     * Computed sizes:
     * n+2*(p) = n + p + 1 + (p - 1)
     * k = (p - 1) values must be removed from sample vector.
     * w = k + 3 window size in moving average
     *
     * Algorithm:
     * 1) compute n - k values using moving average with window size w
     * 2) repeat first and last value p + 1 times
     *
     * The resulting knot vector has n - k + 2*p = n + p + 1 knots.
     *
     * NOTE:
     * For equidistant samples, the resulting knot vector is identically to
     * the free end conditions knot vector used in cubic interpolation.
     * That is, samples (a,b,c,d,e,f) produces the knot vector (a,a,a,a,c,d,f,f,f,f) for p = 3.
     * For p = 1, (a,b,c,d,e,f) becomes (a,a,b,c,d,e,f,f).
     *
     * TODO:
     * Does not work well when number of knots is << number of samples! For such cases
     * almost all knots will lie close to the left samples. Try a bucket approach, where the
     * samples are added to buckets and the knots computed as the average of these.
     */
    std::vector<double> PPForm::Builder::knotVectorMovingAverage(const std::vector<double> &values,
                                                                 unsigned int degree) const
    {
        // Sort and remove duplicates
        std::vector<double> unique = extractUniqueSorted(values);

        // Compute sizes
        unsigned int n = unique.size();
        unsigned int k = degree - 1; // knots to remove
        unsigned int w = k + 3;      // Window size

        // The minimum number of samples from which a free knot vector can be created
        if (n < degree + 1)
        {
            std::ostringstream e;
            e << "knotVectorMovingAverage: Only " << n
              << " unique interpolation points are given. A minimum of degree+1 = " << degree + 1
              << " unique points are required to build a ppForm basis of degree " << degree << ".";
            throw Exception(e.str());
        }

        std::vector<double> knots(n - k - 2, 0);

        // Compute (n-k-2) interior knots using moving average
        for (unsigned int i = 0; i < n - k - 2; ++i)
        {
            double ma = 0;
            for (unsigned int j = 0; j < w; ++j)
                ma += unique.at(i + j);

            knots.at(i) = ma / w;
        }

        // Repeat first knot p + 1 times (for interpolation of start point)
        for (unsigned int i = 0; i < degree + 1; ++i)
            knots.insert(knots.begin(), unique.front());

        // Repeat last knot p + 1 times (for interpolation of end point)
        for (unsigned int i = 0; i < degree + 1; ++i)
            knots.insert(knots.end(), unique.back());

        // Number of knots in a (p+1)-regular knot vector
        // assert(knots.size() == uniqueX.size() + degree + 1);

        return knots;
    }

    std::vector<double> PPForm::Builder::knotVectorEquidistant(const std::vector<double> &values,
                                                               unsigned int degree,
                                                               unsigned int numBasisFunctions = 0) const
    {
        // Sort and remove duplicates
        std::vector<double> unique = extractUniqueSorted(values);

        // Compute sizes
        unsigned int n = unique.size();
        if (numBasisFunctions > 0)
            n = numBasisFunctions;
        unsigned int k = degree - 1; // knots to remove

        // The minimum number of samples from which a free knot vector can be created
        if (n < degree + 1)
        {
            std::ostringstream e;
            e << "knotVectorMovingAverage: Only " << n
              << " unique interpolation points are given. A minimum of degree+1 = " << degree + 1
              << " unique points are required to build a ppForm basis of degree " << degree << ".";
            throw Exception(e.str());
        }

        // Compute (n-k-2) equidistant interior knots
        unsigned int numIntKnots = std::max(n - k - 2, (unsigned int)0);
        numIntKnots = std::min((unsigned int)10, numIntKnots);
        std::vector<double> knots = linspace(unique.front(), unique.back(), numIntKnots);

        // Repeat first knot p + 1 times (for interpolation of start point)
        for (unsigned int i = 0; i < degree; ++i)
            knots.insert(knots.begin(), unique.front());

        // Repeat last knot p + 1 times (for interpolation of end point)
        for (unsigned int i = 0; i < degree; ++i)
            knots.insert(knots.end(), unique.back());

        // Number of knots in a (p+1)-regular knot vector
        // assert(knots.size() == uniqueX.size() + degree + 1);

        return knots;
    }

    std::vector<double> PPForm::Builder::knotVectorBuckets(const std::vector<double> &values, unsigned int degree, unsigned int maxSegments) const
    {
        // Sort and remove duplicates
        std::vector<double> unique = extractUniqueSorted(values);

        // The minimum number of samples from which a free knot vector can be created
        if (unique.size() < degree + 1)
        {
            std::ostringstream e;
            e << "PPForm::Builder::knotVectorBuckets: Only " << unique.size()
              << " unique sample points are given. A minimum of degree+1 = " << degree + 1
              << " unique points are required to build a ppForm basis of degree " << degree << ".";
            throw Exception(e.str());
        }

        // Num internal knots (0 <= ni <= unique.size() - degree - 1)
        unsigned int ni = unique.size() - degree - 1;

        // Num segments
        unsigned int ns = ni + degree + 1;

        // Limit number of segments
        if (ns > maxSegments && maxSegments >= degree + 1)
        {
            ns = maxSegments;
            ni = ns - degree - 1;
        }

        // Num knots
        //        unsigned int nk = ns + degree + 1;

        // Check numbers
        if (ni > unique.size() - degree - 1)
            throw Exception("PPForm::Builder::knotVectorBuckets: Invalid number of internal knots!");

        // Compute window sizes
        unsigned int w = 0;
        if (ni > 0)
            w = std::floor(unique.size() / ni);

        // Residual
        unsigned int res = unique.size() - w * ni;

        // Create array with window sizes
        std::vector<unsigned int> windows(ni, w);

        // Add residual
        for (unsigned int i = 0; i < res; ++i)
            windows.at(i) += 1;

        // Compute internal knots
        std::vector<double> knots(ni, 0);

        // Compute (n-k-2) interior knots using moving average
        unsigned int index = 0;
        for (unsigned int i = 0; i < ni; ++i)
        {
            for (unsigned int j = 0; j < windows.at(i); ++j)
            {
                knots.at(i) += unique.at(index + j);
            }
            knots.at(i) /= windows.at(i);
            index += windows.at(i);
        }

        // Repeat first knot p + 1 times (for interpolation of start point)
        for (unsigned int i = 0; i < degree + 1; ++i)
            knots.insert(knots.begin(), unique.front());

        // Repeat last knot p + 1 times (for interpolation of end point)
        for (unsigned int i = 0; i < degree + 1; ++i)
            knots.insert(knots.end(), unique.back());

        return knots;
    }

    std::vector<double> PPForm::Builder::extractUniqueSorted(const std::vector<double> &values) const
    {
        // Sort and remove duplicates
        std::vector<double> unique(values);
        std::sort(unique.begin(), unique.end());
        std::vector<double>::iterator it = unique_copy(unique.begin(), unique.end(), unique.begin());
        unique.resize(distance(unique.begin(), it));
        return unique;
    }

} // namespace SPLINTER